
#include "weightsScheme.h"

inline Number omega(Number d) {
   
   const Number NINFINITY = std::numeric_limits<Number>::infinity();
   
   if (d==NINFINITY) return 0.00000001;
   if (d==0)         return 1e8;
   else 
      return 1.0/(pow(d,2) + 0.00000001);
}

Number* compute_WeightsTable(Mesh *model, const TConstraints<Point3> *constraints) {
   
   cout<<" compute_WeightsTable(Shape3D *model)"<<endl;
   
   uint texSize = (uint)(std::ceil(std::sqrt(model->nvertices())));
   uint texSizetexSize = texSize*texSize;
   Number *weightsTable = new Number[texSizetexSize*constraints->size()];
   
   for (unsigned j=0; j<constraints->size(); ++j) {
      model->floodfill_Dijkstra(constraints->ids[j]);
      Vertex_iterator vit = model->vertices_begin();
      for (unsigned i=0; i<model->nvertices(); ++i, ++vit) {
         weightsTable[j*texSizetexSize + i] = omega(vit->d);
      }
   }
   
   return weightsTable;
}

Number* compute_WeightsTable_Trihedrons(Mesh *model, const TConstraints<Point3> *constraints) {
   
   cout<<" compute_WeightsTable_Trihedrons(Mesh *model)"<<endl;
   
   uint texSize = (uint)(std::ceil(std::sqrt(model->nvertices())));
   uint texSizetexSize = texSize*texSize;
   Number *weightsTable = new Number[texSizetexSize*constraints->size()];
   
   const Number *a = constraints->getPositionPointer(0);
   const Number *b = constraints->getPositionPointer(1);
   const Vector3 v(Point3(a[0],a[1],a[2]), Point3(b[0],b[1],b[2]));
   const Number d = sqrt(v*v);
   
   for (unsigned j=0; j<constraints->size(); j+=4) {
      model->floodfill_Dijkstra(constraints->ids[j]);
      Vertex_iterator vit = model->vertices_begin();
      for (unsigned i=0; i<model->nvertices(); ++i, ++vit) {
         Number pd = vit->d;
         weightsTable[j*texSizetexSize + i] = omega(pd);
         weightsTable[(j+1)*texSizetexSize + i] = 
         weightsTable[(j+2)*texSizetexSize + i] = 
         weightsTable[(j+3)*texSizetexSize + i] = omega(pd+d);
      }
   }
   
   return weightsTable;
}

Number* compute_WeightsTable(Mesh *model, GSkeleton *skeleton, const TConstraints<Point3> *constraints) {
   
   cout<<" compute_WeightsTable using a distance defined over the skeleton"<<endl;
   uint texSize = (uint)(std::ceil(std::sqrt(model->nvertices())));
   uint texSizetexSize = texSize*texSize;
   Number *weightsTable = new Number[texSizetexSize*constraints->size()];
   
   for (unsigned int j=0; j<constraints->size(); ++j) {
      skeleton->floodfill_Dijkstra(constraints->ids[j]);
      Vertex_iterator vit = model->vertices_begin();
      for (unsigned int i=0; i<model->nvertices(); ++i, ++vit) {
         int icj = vit->rigging_group; /// the closest joint
         assert(icj != -1);
         const Point3 &cj = skeleton->getJointPos(icj);
         const Point3 &p = vit->point();
         Vector3 v = p-cj;
         weightsTable[j*texSizetexSize + i] = omega(skeleton->d[icj] + sqrt(v*v));
      }
   }
   
   cout<<" compute_WeightsTable(Shape with skeleton *model) ... done!"<<endl;
   return weightsTable;
}

/*
Number* compute_WeightsTable(Mesh *model, USkeleton *skeleton, const Constraints *constraints) {
   
   cout<<" compute_WeightsTable using a bones distance ... "<<endl;
   const int &njoints = skeleton->nJoints();
   vector<vector<Number> > d(model->nvertices());
   for (uint i=0; i<d.size(); ++i) {
      d[i].resize(njoints,0.0);
   }
   
   uint ii=0; Number t=0;
   Vertex_iterator vi = model->vertices_begin();
   while (vi != model->vertices_end()) {
      const Point3 &p = vi->point();
      const int &idbone = vi->rigging_group; /// the closest joint
      const pair<int, int> &bone = skeleton->bones[idbone];
      const Point3 &A = skeleton->getJointPos(bone.first);
      const Point3 &B = skeleton->getJointPos(bone.second);
      const Segment3 boneSegment(A,B);
      const Number dbone = skeleton->getDistance(p, boneSegment, t);
      
      const Point3 c = B + t*(A-B); // the closest point over the bone
      const Vector3 cA(c, A);
      const Vector3 cB(c, B);
      const Number dist[2] = {sqrt(cA*cA), sqrt(cB*cB)};
      
      vector<Number> dA(njoints,0.0);
      vector<Number> dB(njoints,0.0);
      skeleton->floodfill_Dijkstra(bone.first);
      for (uint j=0; j<njoints; ++j) dA[j] = skeleton->d[j];
      skeleton->floodfill_Dijkstra(bone.second);
      for (uint j=0; j<njoints; ++j) dB[j] = skeleton->d[j];
      
      for (uint j=0; j<njoints; ++j) {
         const Number djoint = (dA[j]<dB[j])?dist[0]:dist[1]; // distance to the closest joint
         d[ii][j] = dbone + djoint + min(dA[j],dB[j]);
      }
      ++vi; ++ii;
   }
   
   //~ cout<<" printando ......... "<<endl;
   //~ for (uint i=0; i<d.size(); ++i) {
      //~ cout<<" "<<i<<" : " ;
      //~ copy(d[i].begin(), d[i].end(), ostream_iterator<Number>(cout, " "));
      //~ cout<<endl;
   //~ }
   
   cout<<" compute_WeightsTable using a bones distance ... 11"<<endl;
   uint texSize = (uint)(std::ceil(std::sqrt(model->nvertices())));
   uint texSizetexSize = texSize*texSize;
   Number *weightsTable = new Number[texSizetexSize*constraints->size()];
   
   for (unsigned j=0; j<constraints->size(); ++j) {
      for (unsigned i=0; i<model->nvertices(); ++i) {
         weightsTable[j*texSizetexSize + i] = omega(d[i][j]);
      }
   }
   
   cout<<" compute_WeightsTable using a bones distance ... endl"<<endl;
   return weightsTable;
}
*/
